// #include "intr.h"

extern unsigned char PBLAZEPORT[]; // port 


#define SN 7

void main() {
    volatile unsigned char kp  , ki , kd ; // 定义三个变量，用于存储PID参数;
    volatile int target = 0 ; // 定义目标值
    volatile int e0=0;
    volatile int e1=0;
    volatile int e2=0;
    volatile int output = 0 ; // 定义输出值
    volatile int inc = 0 ;
    volatile int current = 0 ; // 定义当前值
    volatile int tmp = 0 ; //

    char rp = 0 ;

    PBLAZEPORT[4] = output&0xff ; // 输出值
    PBLAZEPORT[5] = ((unsigned)output>>8)&0xff ; // 输出值

    // 读取PID参数
    kp = PBLAZEPORT[6] ; // 读取kp
    ki = PBLAZEPORT[7] ; // 读取ki
    kd = PBLAZEPORT[8] ; // 读取kd
    target = PBLAZEPORT[9]|(PBLAZEPORT[10]<<8) ; // 读取目标值
    
    // for(rp=0;rp<10;rp++) {
    for(;;) {
        // 读取当前值
        current = PBLAZEPORT[2]|(PBLAZEPORT[3]<<8) ; // 读取当前值
        // 计算误差
        // e0 = e1 ;
        e1 = e2 ;
        e2 = target - current ; // 计算误差
        inc = 0 ;

        // 计算i增量
        tmp = e2 ; // 计算i增量
        for(rp=0;rp<8;rp++) { // 计算i增量
            if((ki>>rp)&0x1) { // 如果ki的第rp位为1
                inc += tmp>>(SN-rp) ; // 计算i增量
            }
        }

        // 计算p增量 
        tmp = e2 - e1 ; 
        for(rp=0;rp<8;rp++) { // 计算p增量
            if((kp>>rp)&0x1) { // 如果kp的第rp位为1
                inc += tmp>>(SN-rp) ; // 计算p增量
            }
        }


        output += inc ; // 计算输出值
        // output = target/4; 

        // 输出值
        PBLAZEPORT[4] = output&0xff ; // 输出值
        PBLAZEPORT[5] = ((unsigned)output>>8)&0xff ; // 输出值
    }
}



// void delay(int ms)  {
//     int i = 0 ;
//     for(i=0;i<ms;i++){
//         __asm  nop  __endasm; // 空指令， 不做任何操作
//     }
// }
// 中断服务函数 ，名称必须是这个
// void misr()  __interrupt 
// {
//     __asm  DISABLE INTERRUPT  __endasm; // 关中断

//     // __asm  ENABLE INTERRUPT  __endasm; // 开中断 , 可以不动掉，会自动恢复
// }